

#include "Constants.h"
#include "Gamepad.h"
#include "GearBox.h"








/*
 * Default constructor.
 */

GearBox::GearBox (int port, int slot, Gamepad *gamepad, bool isRight)
{
	m_Jaguar1 = new Jaguar(port,slot);
	m_Jaguar2 = new Jaguar(port, slot+1);
	//m_Joystick = joystick;
	m_isRight = isRight;
	m_Encoder = new Encoder(slot+3,slot+4);
	//m_Encoder->SetDistancePerPulse(1.0);  //TODO: set this distance measured in inches
	m_Encoder->Reset();
	m_gamepad = gamepad;

}


GearBox::~GearBox()
{

}

void GearBox::run()
{
	/*
	 * speedToJaguar = GetAxis -> GetLeftY();
	 */
	float speedToJaguar;
	if (m_isRight){
		speedToJaguar = m_gamepad -> GetAxis(GetRightY);
	}
	else{
		speedToJaguar = m_gamepad -> GetAxis(GetLeftY);
	}

		if (m_isHalfSpeed)
		{
		speedToJaguar = speedToJaguar/2;
		}
	drive(speedToJaguar);
}

void GearBox::drive(float speedToJaguar){
	if (m_isRight)
		{
			m_Jaguar1 -> Set(speedToJaguar);
			m_Jaguar2 -> Set(speedToJaguar);
		}
	else
		{
			m_Jaguar1 -> Set(-speedToJaguar);
			m_Jaguar2 -> Set(-speedToJaguar);
		}

}

void GearBox::goHalfSpeed(bool state)
{
	m_isHalfSpeed = state;
}

